import serial #导入模块

import serial.tools.list_ports
import threading
from queue import Queue
import numpy as np


class FrameStatus:

    def readStr(self,strByte):
        if strByte == "":
            return self

        return self.read(int(strByte,16))

    def read(self,intByte):
        None

class StartStatus(FrameStatus):
    def read(self, intByte) -> FrameStatus:

        if intByte == 0x98:
            return LengthStatus()    
        return self

class LengthStatus(FrameStatus):
    def read(self, intByte) -> FrameStatus:
        return PayloadStatus(intByte)

class PayloadStatus(FrameStatus):

    def __init__(self,length) -> None:
        self.length = length
        self.count=0
        self.strList = []



    def read(self, intByte) -> FrameStatus:

        if self.count == self.length:
            return EndStatus(self.strList)   

        self.strList.append(chr(intByte))

        self.count+=1

        return self


class EndStatus(FrameStatus):

    def __init__(self,strList) -> None:

        self.strList = strList

    def read(self, intByte) -> FrameStatus:

        if intByte == 0x14:
            return StartStatus()    
        return self

class SerialComunication(threading.Thread):

    def __init__(self,queue) -> None:
        threading.Thread.__init__(self)
        self.status = StartStatus()
        self.queue = queue

        # 建议使用python读写锁
        self.roll_acc=0
        self.pitch_acc=0
        self.yaw=0

        self.gyr_deg=0
        self.gyr_deg=0
        

    def listUseablePorts(self):
        port_list = list(serial.tools.list_ports.comports())
        print(port_list)
        if len(port_list) == 0:
            print('无可用串口')
            return

        for i in range(0,len(port_list)):
            print(port_list[i])

    def connect(self,port="COM3"):
        self.close()
        self.ser = serial.Serial(port,9600,timeout=1)  # open serial port
        print(self.ser.name)         # check which port was really used
        # ser.write(b'hello')     # write a string

    def run(self) -> None:

            dt = 1/200    
            x_gyr_deg=0
            y_gyr_deg=0
            z_gyr_deg=0
            # 把陀螺仪的角速度从 ADC 量程转换为 度/秒 的 比例系数
            gyr_proportional_coefficient = 0.0076
        #try:
            while(True):
                byte = self.ser.read()
                hexStr = byte.hex()
                newStatus = self.status.readStr(hexStr)
                if  isinstance(self.status,EndStatus):
                    received_str = "".join(self.status.strList)
                    #print(received_str)
                    roll,pitch,yaw,_ = received_str.split(",")
                    roll= float(roll)
                    pitch= float(pitch)
                    yaw = float(yaw) 

                    print("%7f %7f %7f "%(roll,pitch,yaw))
 #                   self.roll_acc = np.rad2deg(np.arctan2(y_acc,z_acc))
 #                   self.pitch_acc = np.rad2deg(- np.arctan2(x_acc,np.sqrt(y_acc**2+z_acc**2)))# -np.arctan2(z_acc,y_acc) #
 #                   
 #                   # 计算陀螺仪旋转角度
 #                   x_gyr_deg += int(x_gyr)*gyr_proportional_coefficient*dt
 #                   y_gyr_deg += int(y_gyr)*gyr_proportional_coefficient*dt
 #                   z_gyr_deg += int(z_gyr)*gyr_proportional_coefficient*dt
 #                   print("%7f %7f %7f %7f %7f %7f" % (self.roll_acc,
 #                                                      self.pitch_acc,
 #                                                      0,
 #                                                      x_gyr_deg,
 #                                                      y_gyr_deg,
 #                                                      z_gyr_deg
 #                                                      ))
                    #self.queue.put_nowait([np.rad2deg(roll),np.rad2deg(pitch),0])
                self.status = newStatus  
        #except serial.SerialException:


    def close(self):          
        if hasattr(self, 'ser') :
            self.ser.close()             # close port    



if __name__ == "__main__":

    q = Queue()
    s = SerialComunication(q)
    s.listUseablePorts()
    s.connect("COM3")
    s.start()

    while True:
        roll,pitch,yaw = q.get()
        print("队列中不应该有内容")





    